#define CATCH_CONFIG_MAIN
#include <catch2/catch_all.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unsupported/Eigen/EulerAngles>

#include <cmath>

using namespace Eigen;
using namespace std;
using Catch::Matchers::WithinAbs;

constexpr double DEG2RAD = 0.01745329251994329576923690768489;
constexpr double RAD2DEG = 57.295779513082320876798154814105;

Matrix3d question1() {
    double angle_x = 30.0 * DEG2RAD;
    Matrix3d R;

    // TODO: 填充旋转矩阵的９个元素
    R(0, 0) = 0.0;
    R(0, 1) = 0.0;
    R(0, 2) = 0.0;
    R(1, 0) = 0.0;
    R(1, 1) = 0.0;
    R(1, 2) = 0.0;
    R(2, 0) = 0.0;
    R(2, 1) = 0.0;
    R(2, 2) = 0.0;

    return R;
}

Matrix3d question2() {
    double phi, omega, kappa;
    phi = 5.0 * DEG2RAD;
    omega = 5.0 * DEG2RAD;
    kappa = 89.0 * DEG2RAD;

    Matrix3d R;

    // TODO: 填充旋转矩阵的９个元素
    R(0, 0) = 0.0;
    R(0, 1) = 0.0;
    R(0, 2) = 0.0;
    R(1, 0) = 0.0;
    R(1, 1) = 0.0;
    R(1, 2) = 0.0;
    R(2, 0) = 0.0;
    R(2, 1) = 0.0;
    R(2, 2) = 0.0;

    return R;
}

Vector3d question3() {
    Matrix3d R = question2();

    // omega'-phi'-kappa'
    // TODO: 填充３个角元素
    Vector3d r;
    r(0) = -0.0;
    r(1) = -0.0;
    r(2) = -0.0;

    return r;
}

Matrix3d golden1() {
    double angle_x = 30.0 * DEG2RAD;
    Matrix3d R = AngleAxisd(angle_x, Vector3d::UnitX()).toRotationMatrix();
    return R;
}

Matrix3d golden2() {
    double phi, omega, kappa;
    phi = 5.0 * DEG2RAD;
    omega = 5.0 * DEG2RAD;
    kappa = 89.0 * DEG2RAD;

    // 教材中的 phi 旋转其实不符合右手法则
    Matrix3d R_phi = AngleAxisd(-phi, Vector3d::UnitY()).toRotationMatrix();
    Matrix3d R_omega = AngleAxisd(omega, Vector3d::UnitX()).toRotationMatrix();
    Matrix3d R_kappa = AngleAxisd(kappa, Vector3d::UnitZ()).toRotationMatrix();

    Matrix3d R = R_phi * R_omega * R_kappa;
    return R;
}

Vector3d golden3() {
    Matrix3d R = golden2();

    using OPKSystem = EulerSystem<-EULER_X, EULER_Y, EULER_Z>;
    using OPK = EulerAngles<double, OPKSystem>;

    OPK opk = OPK(R);
    Vector3d r = opk.angles() * RAD2DEG;
    r(0) = -r(0);
    r(1) = -r(1);
    return r;
}

TEST_CASE("Question 1", "The rotation angle around x axis with 30 degrees") {
    Matrix3d R_student = question1();
    Matrix3d R_golden = golden1();

    REQUIRE_THAT(R_student.determinant(), WithinAbs(1.0, 1e-3));
    REQUIRE_THAT((R_student - R_golden).norm(), WithinAbs(0.0, 1e-3));
}

TEST_CASE("Question 2", "The rotation using phi-omega-kappa system") {
    Matrix3d R_student = question2();
    Matrix3d R_golden = golden2();

    REQUIRE_THAT(R_student.determinant(), WithinAbs(1.0, 1e-3));
    REQUIRE_THAT((R_student - R_golden).norm(), WithinAbs(0.0, 1e-3));
}

TEST_CASE("Question 3", "Conversion between pok and opk") {
    Vector3d r_student = question3();
    Vector3d r_golden = golden3();

    REQUIRE_THAT((r_student - r_golden).norm(), WithinAbs(0.0, 1e-3));
}
